%% 采用DH法计算机械臂的正运动学
%% 输入为 角度 单位是 弧度
function T=FK_DH_S()

syms  q S C real;
syms  qq_1 qq_2 qq_3 qq_4 qq_5 qq_6 real;
syms   a2 a3  real;
syms   S1 S2 S3 S4 S5 S6 C1 C2 C_3 C4 C5 C6 real ;

A1 = DH (qq_1, 0, 0, 0);
A2 = DH (qq_2, 0, a2, pi/2);
A3 = DH (qq_3-pi/2, 0, a3, 0);

T = A1*A2*A3;
T_temp=T;

n=3; %机械臂自由度的个数
% theta 常见的角度组合
SC_simplify=[sin(q) sin(q+pi/2) sin(q-pi/2) cos(q) cos(q+pi/2) cos(q-pi/2)] ;
% theta常见角度的正余弦变换
sc_simplify=[S -C C C -S S];

qq=[qq_1 qq_2 qq_3 qq_4 qq_5 qq_6];

sc_simplify_temp=sc_simplify;

for i=1:1:n
   SC_simplify_temp=subs(SC_simplify,q,qq(i));
    for j=1:1:6
      %%sc_simplify_temp(j)=sprintf('%d%d',sc_simplify(j),i);
      T=subs(T, SC_simplify_temp(j),sc_simplify_temp(j));
    end
end

disp(T);
disp(T_temp);

end



function A = DH(theta_z, d_z, a_x, alpha_x)

A_R_z = [cos(theta_z) -sin(theta_z) 0 0;
         sin(theta_z) cos(theta_z) 0 0;
         0 0 1 0;
         0 0 0 1];

A_T_z = [1 0 0 0;
         0 1 0 0;
         0 0 1 d_z;
         0 0 0 1];

A_T_x = [1 0 0 a_x;
         0 1 0 0;
         0 0 1 0;
         0 0 0 1];

if alpha_x==0
     A_R_x = [1 0 0 0;
         0 1 0  0;
         0 0 1 0;
         0 0 0 1];
 elseif alpha_x==pi/2
       A_R_x = [1 0 0 0;
         0 0 -1  0;
         0 1 0 0;
         0 0 0 1];
 elseif alpha_x==-pi/2
       A_R_x = [1 0 0 0;
         0 0 1  0;
         0 -1 0 0;
         0 0 0 1];
 else
     A_R_x = [1           0             0          0;
         0      cos( alpha_x)     -sin( alpha_x) 0;
         0       sin( alpha_x)    cos( alpha_x) 0;
         0      0                 0           1];
 end

A = A_R_z*A_T_z*A_T_x*A_R_x;
end

